#include <ros/ros.h>
#include <nav_msgs/Path.h>
#include <nav_msgs/Odometry.h>
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/PointStamped.h"
#include"geometry_msgs/Twist.h"
#include "tf/tf.h"
#include <tf/transform_listener.h>
#include<vector>
#include<algorithm>
#include<deque>
#include<math.h>

class LocalPlanner{
public:
    LocalPlanner(ros::NodeHandle& n);

private:
    void init();

    double cal_distance(geometry_msgs::PoseStamped& p1,geometry_msgs::PoseStamped& p2);

    void find_aim_point(geometry_msgs::PointStamped& aim_point);

    double cal_velocity(double distance_to_goal);

    void cal_velocity(geometry_msgs::PointStamped& aim_point,double distance_to_goal);

    void pub_speed(double spd_x,double spd_y);

    void plan();

    void path_callback(const nav_msgs::PathConstPtr& msg);

    void odom_callback(const nav_msgs::OdometryConstPtr& msg);

private:
    ros::Publisher vel_pub;
    ros::Subscriber path_sub;
    ros::Subscriber odom_sub;
    std::shared_ptr<tf::TransformListener> listener_ptr;

    geometry_msgs::PoseStamped current_pose;
    std::vector<geometry_msgs::PoseStamped> global_path;
    geometry_msgs::Twist fdb_vel;
    double p_parameter;
    double a_max;      
    double v_max;
    int planner_rate;
    double arrive_error; 
    double aim_distance;

    enum planner_status_enum{
        PLANNING = 1,
        WAIT,
        FAILURE,
        STOP
    };

    planner_status_enum planner_status;




};